﻿using Microsoft.Xna.Framework;

namespace Phoenix.Xna.Framework.FrameworkExtensions
{
    public static class QuaternionExtensions
    {
        public static Vector3 ToEulerAngle(this Quaternion obj)
        {
            Vector3 forward = Vector3.Transform(Vector3.Forward, obj);
            Vector3 up = Vector3.Transform(Vector3.Up, obj);
            Vector3 rotationAxis = new Vector3().AngleTo(forward);

            if (rotationAxis.X == MathHelper.PiOver2)
            {
                rotationAxis.Y = (float)System.Math.Atan2(up.X, up.Z);
                rotationAxis.Z = 0;
            }
            else if (rotationAxis.X == -MathHelper.PiOver2)
            {
                rotationAxis.Y = (float)System.Math.Atan2(-up.X, -up.Z);
                rotationAxis.Z = 0;
            }
            else
            {
                up = Vector3.Transform(up, Matrix.CreateRotationY(-rotationAxis.Y));
                up = Vector3.Transform(up, Matrix.CreateRotationX(-rotationAxis.X));
                rotationAxis.Z = (float)System.Math.Atan2(-up.Z, up.Y);
            }

            return rotationAxis;
        }
    }
}
